/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
	APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor
	Code by Jordi Mu�oz and Jose Julio. DIYDrones.com

	This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

	Sensor is conected to I2C port
	Sensor End of Conversion (EOC) pin is PC7 (30)

	Variables:
		RawTemp : Raw temperature data
		RawPress : Raw pressure data

		Temp : Calculated temperature (in 0.1�C units)
		Press : Calculated pressure   (in Pa units)

	Methods:
		Init() : Initialization of I2C and read sensor calibration data
		Read() : Read sensor data and calculate Temperature and Pressure
		         This function is optimized so the main host don�t need to wait
				 You can call this function in your main loop
				 It returns a 1 if there are new data.

	Internal functions:
		Command_ReadTemp(): Send commando to read temperature
		Command_ReadPress(): Send commando to read Pressure
		ReadTemp() : Read temp register
		ReadPress() : Read press register
		Calculate() : Calculate Temperature and Pressure in real units


*/

#include <includes.h>
//#include "i2c2_drv.h"
#include "I2CRoutines.h"
#include "AP_Baro_BMP085.h"
#include "AppTimer.h"
#include "MyRtos.h"
#include "Modbus.h"

#define BMP085_ADDRESS 0x77  //(0xEE >> 1)
#define BMP085_EOC 30        // End of conversion pin PC7

#define WaitBMP05Cnv()         while(!BMP_DATA_READY()){Yield();}


// the apm2 hardware needs to check the state of the
// chip using a direct IO port
// On APM2 prerelease hw, the data ready port is hooked up to PE7, which
// is not available to the arduino digitalRead function.

#define BMP085_EOC_PIN   GPIO_Pin_14
#define BMP_DATA_READY() GPIO_ReadInputDataBit(GPIOC, BMP085_EOC_PIN)


// Public Methods //////////////////////////////////////////////////////////////
void AP_Baro_BMP085::init( void)
{
  GPIO_InitTypeDef GPIO_InitStructure;
        // Setup ECO pin as input
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
  GPIO_InitStructure.GPIO_Pin = BMP085_EOC_PIN;
  GPIO_Init(GPIOC, &GPIO_InitStructure);
        
  PressState = 0;		 // Initial state
  TempState = 0;
  _cm_Offset = 0;
  _Pa_Offset = 0;          // 1hPa = 100Pa = 1mbar        
  I2C2_Open();
  _param_datum = 101325;   // sea level p0   
 // We read the calibration data registers

   GetCalData();

 // init raw temo
   RawTemp = 0;

   healthy = true;
   I2C2_Close();
   BMP085_in_Used = 0;     
}

#if 0
void AP_Baro_BMP085::init(int32_t _initVal, bool _Unitmeters){     
  init();               
  calcTrueTemperature();      // initialize b5
  _Unitmeters ? setLocalAbsAlt(_initVal) : setLocalPressure(_initVal); 
}
#endif
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
void AP_Baro_BMP085::GetCalData(void)
{
  uint8_t buff[22];
 
  if(I2C2_Read_Registers(BMP085_ADDRESS,CAL_AC1,buff,  22 )!=Success) return;
  ac1 = ((int)buff[0] << 8) | buff[1];
  ac2 = ((int)buff[2] << 8) | buff[3];
  ac3 = ((int)buff[4] << 8) | buff[5];
  ac4 = ((int)buff[6] << 8) | buff[7];
  ac5 = ((int)buff[8] << 8) | buff[9];
  ac6 = ((int)buff[10] << 8) | buff[11];
  b1 = ((int)buff[12] << 8) | buff[13];
  b2 = ((int)buff[14] << 8) | buff[15];
  mb = ((int)buff[16] << 8) | buff[17];
  mc = ((int)buff[18] << 8) | buff[19];
  md = ((int)buff[20] << 8) | buff[21];
}
#if 0
void AP_Baro_BMP085::setLocalPressure(int32_t _Pa){   
  int32_t tmp_alt;
 
  _param_datum = _Pa;       // reference point pressure, p0
  getAlltitude(&tmp_alt);    // calc altitude based on current pressure   
  _param_centimeters = tmp_alt; // reference point altitude
}

void AP_Baro_BMP085::setLocalAbsAlt(int32_t _centimeters){  
  int32_t tmp_Pa;
 
  _param_centimeters = _centimeters;   // reference point altitude
  getPressure(&tmp_Pa);    // calc pressure based on current altitude
  _param_datum = tmp_Pa;   // p0
}

void AP_Baro_BMP085::setAltOffset(int32_t _centimeters){
  _cm_Offset = _centimeters;
}

void AP_Baro_BMP0855::sethPaOffset(int32_t _Pa){
  _Pa_Offset = _Pa;
}

void AP_Baro_BMP085::zeroCal(int32_t _Pa, int32_t _centimeters){
  setAltOffset(_centimeters - _param_centimeters);    
  sethPaOffset(_Pa - _param_datum);    
}
#endif

// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
void AP_Baro_BMP085::RefreshData(void)
{
  UpdateTemp();
  UpdatePress();
}

int32_t AP_Baro_BMP085::get_pressure(void) {
    return Press;
}

int16_t AP_Baro_BMP085::get_temperature(void) {
    return Temp;
}

int32_t AP_Baro_BMP085::get_altitude(void) { // unit in centi-meter
  return Altitude;  
}

void AP_Baro_BMP085::CalculateAltitude(uint32_t PressIn) {
  Altitude =  (int32_t)(4433000 * (1 - pow( ((float)PressIn / (float)_param_datum), (float)0.1903)) + _cm_Offset); // unit in CM
  Reg4xxxx[ALTITUDE_REG] = Altitude;
}


int32_t AP_Baro_BMP085::get_raw_pressure(void) {
    return RawPress;
}

int32_t AP_Baro_BMP085::get_raw_temp(void) {
    return RawTemp;
}

// Private functions: /////////////////////////////////////////////////////////

static    uint8_t buf[3];

// Read Raw Pressure values every 1 Sec

void AP_Baro_BMP085::UpdatePress(void)
{
    if(PressState == 0){
       PressTimer = SetTimer(0);
       PressState = 1;      
    }
    if(ChkTimer(PressTimer)){ // wait for conversion time up
      return;
    }
    PressTimer += PRESS_ACQ_TIME;
    // Wait I2C2 free
    WaitI2C2(); 
    // send start presure conversion command
    buf[0] = 0xf4;
    buf[1] = 0x34+(OVERSAMPLING << 6);
    if(I2C2_Master_BufferWrite(buf,2, BMP085_ADDRESS)!= Success) { // if fail
        I2C2_Close();
        return;
    }
    I2C2_Close();
    Sleep(15); // sleep 15 ms
    
    WaitBMP05Cnv(); // wait conversion done
    WaitI2C2();     // wait I2C2 free
    
    if(I2C2_Read_Registers(BMP085_ADDRESS,0xf6,buf,3)!= Success) {
          I2C2_Close();
          return;
    }        
    I2C2_Close();
    // get Presure data
    RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
    CalculatePress(RawPress);  // convert raw press data to real press
    CalculateAltitude(Press);  // convert from real press to altitude   
}

// Read Raw Temperature values every 1 Sec
void AP_Baro_BMP085::UpdateTemp(void)
{
    int32_t _temp_sensor;

    if(TempState == 0){
       TempTimer = SetTimer(0);
       TempState = 1;      
    }
    if(ChkTimer(TempTimer)){ // wait for conversion time up
      return;
    }
    TempTimer += TEMP_ACQ_TIME;
    
    WaitI2C2();     // wait I2C2 free
    // start convertion command
    buf[0] = 0xf4;
    buf[1] = 0x2E;
    if(I2C2_Master_BufferWrite(buf,2, BMP085_ADDRESS)!= Success) {
        I2C2_Close();
        return;
    }           
    FreeI2C2();
    WaitBMP05Cnv(); // wait conversion done
    WaitI2C2();     // wait I2C2 free
    if(I2C2_Read_Registers(BMP085_ADDRESS,0xf6,buf,2)!= Success) {
          I2C2_Close();
          return;
    }        
    FreeI2C2();   
    _temp_sensor = buf[0];
    _temp_sensor = (_temp_sensor << 8) | buf[1];
    RawTemp = _temp_sensor;
    calcTrueTemperature();  
}

void AP_Baro_BMP085::calcTrueTemperature(void){
  int32_t x1,x2;

// See Datasheet page 13 for this formulas
// Based also on Jee Labs BMP085 example code. Thanks for share.
// Temperature calculations
  x1 = ((int32_t)RawTemp - ac6) * ac5 >> 15;
  x2 = ((int32_t) mc << 11) / (x1 + md);
  b5 = x1 + x2;   // update b5
  Temp = (b5 + 8) >> 4;
  Reg4xxxx[TEMPERATURE_REG] = Temp;
}

// Calculate Pressure in real units.
void AP_Baro_BMP085::CalculatePress(int32_t PressRaw)
{
	int32_t x1, x2, x3, b3, b6, p;
	uint32_t b4, b7;
	int32_t tmp,Press1;

	// Pressure calculations
	b6 = b5 - 4000;
	x1 = (b2 * (b6 * b6 >> 12)) >> 11;
	x2 = ac2 * b6 >> 11;
	x3 = x1 + x2;
	//b3 = (((int32_t) ac1 * 4 + x3)<<OVERSAMPLING + 2) >> 2; // BAD
	//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2;  //OK for OVERSAMPLING=0
	tmp = ac1;
	tmp = (tmp*4 + x3)<<OVERSAMPLING;
	b3 = (tmp+2)/4;
	x1 = ac3 * b6 >> 13;
	x2 = (b1 * (b6 * b6 >> 12)) >> 16;
	x3 = ((x1 + x2) + 2) >> 2;
	b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
	b7 = ((uint32_t) PressRaw - b3) * (50000 >> OVERSAMPLING);
	p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;

	x1 = (p >> 8) * (p >> 8);
	x1 = (x1 * 3038) >> 16;
	x2 = (-7357 * p) >> 16;
	Press1 = p + ((x1 + x2 + 3791) >> 4);
        Press = ((Press*7)+Press1)>>3;
        Reg4xxxx[PRESURE_REG_HI] = Press>>16;
        Reg4xxxx[PRESURE_REG_LO] = Press;        
}
